
import os
os.environ["KMP_DUPLICATE_LIB_OK"] = "TRUE"
import threading

from tools import *
import hqyj_mqtt
import base64
import numpy as np
import cv2
import time
from pid import PID
from pydantic import BaseModel
from fastapi import FastAPI, HTTPException
import uvicorn
app = FastAPI()

def b64_to_np(image):
    # 此时的image还是一个字典，我们需要的是字典的值，字典的值是base64格式的图像数据
    img_data = base64.b64decode(image['image'])

    # 将字节数组转换numpy数组
    img_np = np.frombuffer(img_data, dtype=np.uint8, count=len(img_data))

    # 使用opencv读取该数组
    img = cv2.imdecode(img_np, cv2.IMREAD_COLOR)

    return img

#获取坐标，矩阵变换
def perspective_transform(image):
    # 图像的宽度高度
    image_size = (image.shape[1], image.shape[0])
    # 定义源图像中要变换的四边形顶点坐标，用于提取感兴趣的车道区域
    src = np.float32(
        [[80, image_size[1]],
         [450, image_size[1]],
         [image_size[0] // 2 + 40, image_size[1] // 2 - 20],
         [image_size[0] // 2 - 40, image_size[1] // 2 - 20]]
    )
    # 定义目标图像中变换后的四边形顶点坐标，将源区域转换为鸟瞰图视角
    dst = np.float32(
        [[image_size[0] / 4, image_size[1]],
         [image_size[0] * 3 / 4, image_size[1]],
         [image_size[0] * 3 / 4, 0],
         [image_size[0] / 4, 0]]
    )
    # 获取透视变换矩阵
    M = cv2.getPerspectiveTransform(src, dst)
    #获取逆透视变换矩阵
    minv = cv2.getPerspectiveTransform(dst, src)
    # 调用函数进行透视变换，flags是像素插值的方法，这里是线性插值法
    image_warp = cv2.warpPerspective(image, M, image_size, flags=cv2.INTER_LINEAR)
    return image_warp,minv


#膨胀腐蚀，让有些断掉的白色像素链接起来
def dilate_erode(image,kernel_size):
    kernel = np.ones((kernel_size, kernel_size), np.uint8)
    #先膨胀后腐蚀操作
    dilation = cv2.dilate(image, kernel, iterations=1)
    erosion = cv2.erode(dilation, kernel, iterations=1)
    return erosion
#提取车道线1，梯度方法,把矫正之后的图像中的车道线提取出来：
def extract_line_gradient(image_warp):
    #第一步，对传进来的图进行一次滤波
    img_Gaussian = cv2.GaussianBlur(image_warp, (5, 5), 1)

    #灰度化
    img_gray = cv2.cvtColor(img_Gaussian, cv2.COLOR_BGR2GRAY)

    #使用sobel算子进行梯度的算法
    res = cv2.Sobel(img_gray, -1, 1, 0)

    #二值化
    tet, image_binary = cv2.threshold(res,127,255,cv2.THRESH_BINARY)

    res=dilate_erode(image_binary, 15)

    return res

#发现左右,拟合车道线
def finding_line(dilate_erode_image):
    #由于车是向前走的，在图像中表示车是向上走的，所以通常更关注黑白图的下半部分
    histogram   = np.sum(dilate_erode_image[dilate_erode_image.shape[0]//2:,:],axis=0)
    # plt.plot(histogram)
    # plt.show()

    #创建一个三通道图像，显示小窗口
    out_img = np.dstack((dilate_erode_image, dilate_erode_image, dilate_erode_image))

    #获取直方图的中点位置，就是图像的一半，因为获取最大值坐标只能获取一个
    midpoint = histogram.shape[0]//2

    #获取左侧最大值的坐标
    leftx_base= np.argmax(histogram[:midpoint])
    #获取右侧最大值的坐标
    rightx_base = np.argmax(histogram[midpoint:])+midpoint

    #根据两个点，获取左右两边的白像素
    #获取图像中所有非零像素的x和y的位置,返回行索引和列索引
    nonzero = dilate_erode_image.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])

    #定义一些小窗口的概念
    #定义小窗口个数
    nwindows = 10
    #小窗口的高度
    window_height = dilate_erode_image.shape[0]//nwindows
    #小窗口的宽度
    margin = 50

    #小窗口白色像素点的个数阈值
    minpix = 20

    #初始化当前窗口的位置，后面持续更新
    leftx_current = leftx_base
    rightx_current = rightx_base

    leftx_pre = leftx_current
    rightx_pre = rightx_current


    #创建空列表去接收左右两侧的像素索引
    left_lane_inds = []
    right_lane_inds = []

    for window in range(nwindows):
        #计算当前窗口的上边界的y坐标
        win_y_high = dilate_erode_image.shape[0] - (window+1)*window_height

        #计算当前窗口的下边界的y坐标
        win_y_low = dilate_erode_image.shape[0] - (window)*window_height

        #计算左边窗口左右边界的x坐标,中心坐标加上宽度就是往两边各扩一个宽度，方块长100
        win_xleft_low = leftx_current - margin
        win_xleft_high = leftx_current + margin

        #计算右边窗口左右边界的x坐标
        win_rightx_low = rightx_current - margin
        win_rightx_high = rightx_current + margin

        #在out_img里显示小窗口滑动过程
        cv2.rectangle(out_img,(win_xleft_low,win_y_high),(win_xleft_high,win_y_low),(0,255,0),2)
        cv2.rectangle(out_img, (win_rightx_low, win_y_high), (win_rightx_high, win_y_low), (0, 255, 0), 2)
        # cv2.imshow("window", out_img)

        #找到处于窗口内非零像素的索引
        good_left_inds = ((nonzeroy>=win_y_high)&(nonzeroy<win_y_low)&(nonzerox >= win_xleft_low)&(nonzerox<win_xleft_high)).nonzero()[0]
        good_right_inds = ((nonzeroy >= win_y_high) & (nonzeroy < win_y_low) & (nonzerox >= win_rightx_low) & (
                    nonzerox < win_rightx_high)).nonzero()[0]

        #将获取到的白色像素的索引存入列表
        left_lane_inds.append(good_left_inds)
        right_lane_inds.append(good_right_inds)

        #更新小窗口的位置
        if len(good_left_inds) > minpix:
            leftx_current = int(np.mean(nonzerox[good_left_inds]))
        else:

            if len(good_right_inds) > minpix:
                offset = int(np.mean(nonzerox[good_right_inds]))-rightx_pre
                leftx_current = leftx_current+offset

        if len(good_right_inds) > minpix:
            rightx_current = int(np.mean(nonzerox[good_right_inds]))
        else:
            if len(good_left_inds) > minpix:
                offset = int(np.mean(nonzerox[good_left_inds]))-leftx_pre
                rightx_current=rightx_current+offset

        #记录上一次的位置
        leftx_pre = leftx_current
        rightx_pre = rightx_current

    #连接索引的列表，为了后续更方便的提取出这些像素点的x和y的坐标，以便进行车道线的拟合
    left_lane_inds = np.concatenate(left_lane_inds)
    right_lane_inds = np.concatenate(right_lane_inds)

    #提取左侧和右侧车道线像素的位置，left——lane——inds是一个一维数组，他包含了左侧车道线在滑动窗口中找到的白色像素点的x坐标的索引
    #通过将这些索引作为索引器应用到nonzerox数组上，就可以得到相应的左侧车道线的x坐标
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]

    #有了坐标之后，就要去车道拟合
    #np.polyfit()是numpy重用于进行多项模式拟合的函数
    #他接受三个参数：x ,y,deg
    #x:自变量数组，y:因变量数组，deg多项式的次数,如果是2，y=ax^2+b^x+c
    #left_fit里存放的就是a,b,c的参数，因为不同位置
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)

    #生成一组均匀分布的数值，用于表示竖直方向的像素坐标，方便后续的车道线绘制
    ploty = np.linspace(0,dilate_erode_image.shape[0]-1,dilate_erode_image.shape[0])

    #使用多项式拟合来估计左侧和右侧车道线的x坐标
    left_fitx = left_fit[0]*ploty**2+left_fit[1]*ploty+left_fit[2]
    right_fitx = right_fit[0]*ploty**2+right_fit[1]*ploty+right_fit[2]

    #计算中间车道线的位置
    middle_fitx = (left_fitx+right_fitx)//2

    #使用不同颜色将车道线标出来
    out_img[lefty, leftx] = [255, 0, 0]
    out_img[righty, rightx] = [ 0, 0,255]

    # cv2.imshow('out_img', out_img)

    return left_fitx, right_fitx, middle_fitx,ploty

#绘制车道线
def show_line(left_fitx,image, image_warp,dilate_erode_image,minv, right_fitx, middle_fitx,ploty):
    #创建一个空白图像，用于在上面绘制车道线，显示四个图像
    warp_zero =  np.zeros_like(dilate_erode_image).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))

    #组合车道线坐标
    # print(left_fitx.shape)
    pts_left = np.transpose(np.vstack([left_fitx,ploty]))
    # print(pts_left)
    pts_right = np.transpose(np.vstack([right_fitx,ploty]))
    pts_middle = np.transpose(np.vstack([middle_fitx,ploty]))

    #绘制车道线
    cv2.polylines(color_warp, np.int32([pts_left]), isClosed=False, color=(255,124,0),thickness=15)
    cv2.polylines(color_warp, np.int32([pts_right]), isClosed=False, color=(255, 124, 0), thickness=15)
    cv2.polylines(color_warp, np.int32([pts_middle]), isClosed=False, color=(255, 124, 0), thickness=15)
    # cv2.imshow("color_warp", color_warp)

    #将得到的车道线的像素点根据逆透视变换矩阵映射到原始图像
    newwarp = cv2.warpPerspective(color_warp,minv,(image_warp.shape[1],image_warp.shape[0]))

    result1 = cv2.addWeighted(image, 1, newwarp, 1, 0)
    # cv2.imshow("result1", result1)

    #创建一个灰色图
    background = np.zeros_like(image).astype(np.uint8)+127
    result = cv2.addWeighted(background, 1, newwarp, 1, 0)
    # cv2.imshow("result", result)

    #拼接成一个窗口显示四个图
    concatenate_imagex = np.concatenate((image,image_warp), axis=1)
    concatenate_imagex2 = np.concatenate((result1,result),axis=1)
    concatenate_imagey = np.concatenate((concatenate_imagex,concatenate_imagex2),axis=0)
    cv2.imshow('concatenate_image', concatenate_imagey)
    cv2.waitKey(1)

    return pts_middle

#自动驾驶,利用pid实现
def auto_run(image,mqtt_client,pts_middle,pid,carspeed = 10):
    #计算车道中心的像素坐标,目标位置（实际是【当前车道中心】（车子当前所在位置）本项目中互换了）
    lane_center = pts_middle[240:,:].mean()
    #当前位置（其实是目标位置,（希望车子去的图像中心））
    image_center = image.shape[1]//2

    # print(pts_middle)
    steering_angle  = -pid(lane_center)

    print(f"当前车道中心: {lane_center:.1f}, 目标中心: {image_center:.1f}, 转向角: {steering_angle:.1f}")

    #发送控制指令
    control_cmd = {
        "carSpeed": carspeed,  # 车速
        "carDirection": steering_angle  # 转向角
    }
    mqtt_client.control_device(control_cmd )


    return lane_center,image_center
#红绿灯识别
def find_stop_line(image_arpPerspective):
    global stop_line_flag
    stop_line_flag = False
    hsv = cv2.cvtColor(image_arpPerspective, cv2.COLOR_BGR2HSV)
    white_low = [0, 0, 200]
    white_up = [180, 30, 255]
    mask = cv2.inRange(hsv, np.array(white_low), np.array(white_up))
    # 进行闭操作
    img_close = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, np.ones((15, 15), np.uint8))
    Contours, hierarchy = cv2.findContours(img_close, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if len(Contours) > 0:
        # 取出面积最大的轮廓
        Contour = max(Contours, key=cv2.contourArea)
        Area  = cv2.contourArea(Contour)
        if Area>2000:
            print('发现车道线，车道线面积：',Area)
            # 通过boundingRect获取当前轮廓点所构成的外接矩形的左上角的点的坐标及外接矩形的宽度和高度
            x, y, w, h = cv2.boundingRect(Contour)
            # 此时的(x,y)就是左上角的点的坐标 w是矩形的宽度 h是矩形的高度
            top_left = (x, y)
            # 右下角的点的坐标
            bottom_right = (x + w, y + h)
            # 通过rectangle去绘制矩形
            # cv2.rectangle(image_arpPerspective, top_left, bottom_right, (255, 0, 0), 1)
            cv2.drawContours(image_arpPerspective, [Contour], -1, (0, 0, 255), 1)

            # 车与停止线的偏差
            offset = 20
            if bottom_right[1] > image_arpPerspective.shape[0] - offset:
                print('停车线已到')
                stop_line_flag = True


    # cv2.imshow('image_arpPerspective_stop',image_arpPerspective)
    # cv2.imshow('image_binary_stop',img_close)
    return stop_line_flag

#fastapi


def image_to_base64(image):
    """将OpenCV图像转换为base64字符串"""
    _, encoded_img = cv2.imencode('.png', image)
    if not _:
        raise ValueError("图像编码失败")
    return base64.b64encode(encoded_img).decode('utf-8')





# 注意：需放在main.py中已有的导入和函数定义之后

class ImageRequest(BaseModel):
    image: str  # 接收base64编码的图像字符串


@app.post("/process-image")
async def process_image(req: ImageRequest):
    try:
        # 1. 解码Base64图像为OpenCV格式
        img_data = base64.b64decode(req.image)
        img_np = np.frombuffer(img_data, dtype=np.uint8)
        img = cv2.imdecode(img_np, cv2.IMREAD_COLOR)

        if img is None:
            raise HTTPException(status_code=400, detail="无法解码图像数据")

        # 2. 执行车道线检测流程
        image_warp, minv = perspective_transform(img)  # 透视变换
        grad_img = extract_line_gradient(image_warp)  # 提取梯度特征
        left_fitx, right_fitx, middle_fitx, ploty = finding_line(grad_img)  # 拟合车道线

        # 3. 绘制车道线并拼接图像（完善show_line函数的返回逻辑）
        # 创建空白图像用于绘制车道线
        warp_zero = np.zeros_like(grad_img).astype(np.uint8)
        color_warp = np.dstack((warp_zero, warp_zero, warp_zero))

        # 组合车道线坐标并绘制
        pts_left = np.transpose(np.vstack([left_fitx, ploty]))
        pts_right = np.transpose(np.vstack([right_fitx, ploty]))
        pts_middle = np.transpose(np.vstack([middle_fitx, ploty]))
        cv2.polylines(color_warp, np.int32([pts_left]), isClosed=False, color=(255, 124, 0), thickness=15)
        cv2.polylines(color_warp, np.int32([pts_right]), isClosed=False, color=(255, 124, 0), thickness=15)
        cv2.polylines(color_warp, np.int32([pts_middle]), isClosed=False, color=(0, 255, 255), thickness=10)

        # 逆透视变换，将车道线映射回原始图像
        newwarp = cv2.warpPerspective(color_warp, minv, (image_warp.shape[1], image_warp.shape[0]))

        # 图像融合（原始图像+车道线）
        result1 = cv2.addWeighted(img, 1, newwarp, 1, 0)

        # 创建辅助显示图像（灰色背景+车道线）
        background = np.zeros_like(img).astype(np.uint8) + 127  # 灰色背景
        result = cv2.addWeighted(background, 1, newwarp, 1, 0)

        # 拼接4张图像（原始图+变换图+融合图+辅助图）
        concatenate_imagex = np.concatenate((img, image_warp), axis=1)  # 水平拼接上半部分
        concatenate_imagex2 = np.concatenate((result1, result), axis=1)  # 水平拼接下半部分
        final_image = np.concatenate((concatenate_imagex, concatenate_imagex2), axis=0)  # 垂直拼接整体

        # 4. 编码处理结果为Base64返回
        _, buffer = cv2.imencode('.jpg', final_image)
        img_b64 = base64.b64encode(buffer).decode()

        return {"status": "success", "result": img_b64}

    except Exception as e:
        return {"status": "error", "error": str(e)}


if __name__ == '__main__':

    # 1. 构建mqtt客户端，连接mqtt服务器，便和3D场景通信
    mqtt_client = hqyj_mqtt.Mqtt_Clt('127.0.0.1', 21883, 'bb', 'aa', 60)


    #为调用pid做准备,实现自动驾驶
    frame_count = 0
    start_time = time.time()

    pid = PID(Kp=0.28, Ki=0.01, Kd=0.1,setpoint= 240)
    pid.sample_time = 0.1
    pid.output_limits = (-13,13)


    def run_server():
        uvicorn.run(app, host="127.0.0.1", port=8080)

    #守护线程：daemon = True确保主程序退出时线程自动结束
    threading.Thread(target=run_server, daemon=True).start()

    while True:
        # try:
        image = mqtt_client.mqtt_queue.get()
        if 'image' in image:

            # 将接收到的消息解析为opencv处理能用的格式
            image = b64_to_np(image)

            """第一步图像矫正"""
           # 对图像进行透视变换
            image_warp,minv= perspective_transform(image)
            # image_warp_copy = image_warp.copy()

            """第二步车道线提取"""
            #提取车道线
            #方法一：使用梯度提取车道线,仅仅只能确定是车道线，不能确定他是车子所在车道线
            dilate_erode_image= extract_line_gradient(image_warp)

            """车道线优化，获取我要的车道线,拟合车道线"""
            left_fitx, right_fitx, middle_fitx,ploty=finding_line(dilate_erode_image)

            """绘制车道线"""
            pts_middle= show_line(left_fitx,image,  image_warp, dilate_erode_image, minv, right_fitx, middle_fitx, ploty)

            """红绿灯检测"""
            stop_line_flag = find_stop_line(image_warp.copy())
            light_color, person_Warning = yolo_detect(image)
            if person_Warning == True:
                print('前方检测到行人，，需要刹车！！！')
                mqtt_client.control_device({"carSpeed": 0})
                time.sleep(1)
                continue
            if stop_line_flag == True:  # 停止的状态
                mqtt_client.control_device({"carSpeed": 0})
                time.sleep(1)

                print('识别到的颜色为:', light_color)
                if light_color != None:
                    if light_color == 0 or light_color == 1:

                        print('停止车辆')
                        mqtt_client.control_device({"carSpeed": 0})
                        cv2.waitKey(1)
                        continue
                    elif light_color == 2 or light_color == None:
                        print('绿灯，启动车辆')
                        mqtt_client.control_device({"carSpeed": 10})
                        time.sleep(4)

            """自动驾驶功能"""
            lane_center,image_center = auto_run(image,mqtt_client,pts_middle,pid)

    cv2.destroyAllWindows()
      # except Exception as e:
        #     print(e)



